#include "ComponentPilot.h"
#include "Components/ComponentCreature.h"
#include "Components/ComponentBody.h"
#include "Components/ComponentLocomotion.h"

#include "Managers/GameManager.h"
#include "GameRandom.hpp"
#include "GameRegistry.hpp"
#include "Segment2.h"
#include "Subsystems/SubsystemTimeOfDay.h"
#include "Subsystems/SubsystemBodies.h"
#include "Subsystems/SubsystemTerrain.h"
#include <cfloat>
#include "Managers/BlocksManager.h"

#include "Blocks/WaterBlock.h"
#include "Log.h"
#include "GameSingleton.h"
#include "GameThreadPool.hpp"



namespace PocketSurvival
{
    static std::shared_ptr<Vector2> CombineNullables(std::shared_ptr<Vector2> &v1, std::shared_ptr<Vector2> &v2);
    static std::shared_ptr<Vector3> CombineNullables(std::shared_ptr<Vector3> &v1, std::shared_ptr<Vector3> &v2);

    void ComponentPilot::update(float dt)
    {
        SubsystemTimeOfDay *timeOfDay = GameSingleton::singletonPtr->getSingleton<SubsystemTimeOfDay>();
        SubsystemTerrain *terrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();

        ComponentBody *componentBody = componentCreature->componentBody;
        ComponentLocomotion *componentLocomotion = componentCreature->componentLocomotion;

        if (timeOfDay->getGameTime() >= nextUpdateTime)
        {
            nextUpdateTime = timeOfDay->getGameTime() + GameSingleton::gameRandom->getFloat(0.09f, 0.11f);
            walkOrder = nullptr;
            flyOrder = nullptr;
            swimOrder = nullptr;
            turnOrder = FastVector2::Zero;
            jumpOrder = 0.0f;

            if (destination != FastVector3::Zero)
            {
                Vector3 position;
                componentBody->getPosition(position);

                Vector3 forward = componentBody->getMatrix().getForward();
                Vector3 v = avoidNearestBody(position, destination);
                Vector3 vector = v - position;
                float num = vector.lengthSquared();
                Vector2 vector2(v.posX - position.posX, v.posZ - position.posZ);
                float num2 = vector2.lengthSquared();
                float x = Vector2::Angle(Vector2(forward.posX, forward.posZ), Vector2(vector.posX, vector.posZ));
                float num3 = ((componentBody->collisionVelocityChange * Vector3(1.0f, 0.0f, 1.0f)).lengthSquared() > 0.0f && componentBody->standingOnValue != 0) ? 0.15f : 0.4f;
                
                if (timeOfDay->getGameTime() >= lastStuckCheckTime + num3 || lastStuckCheckPosition == nullptr)
                {
                    lastStuckCheckTime = timeOfDay->getGameTime();

                    if (MathUtils::Abs(x) > MathUtils::DegToRad(20.0f) || lastStuckCheckPosition == nullptr || Vector3::Dot(position - *(lastStuckCheckPosition), Vector3::Normalize(vector)) > 0.2f)
                    {
                        lastStuckCheckPosition = std::make_shared<Vector3>(position);
                        stuckCount = 0;
                    }
                    else
                    {
                        stuckCount++;
                    }
                    isStuck = (stuckCount >= 4);
                }
                if (componentLocomotion->flySpeed > 0.0f && (num > 9.0f || vector.posY > 0.5f || vector.posY < -1.5f || (componentBody->standingOnValue == 0 && componentBody->immersionFactor == 0.0f)) && componentBody->immersionFactor < 1.0f)
                {
                    float y = MathUtils::Min<float>(0.08f * vector2.lengthSquared(), 12.0f);
                    Vector3 v2 = v + Vector3(0.0f, y, 0.0f);

                    std::shared_ptr<Vector3> value2 = std::make_shared<Vector3>(speed * Vector3::Normalize(v2 - position));
                    value2->posY = MathUtils::Max<float>(value2->posY, -0.5f);

                    flyOrder = value2;
                    turnOrder = Vector2(MathUtils::Clamp(x, -1.0f, 1.0f), 0.0f);
                }
                else if (componentLocomotion->swimSpeed > 0.0f && componentBody->immersionFactor > 0.5f)
                {
                    Vector3 value3 = speed * Vector3::Normalize(v - position);
                    value3.posY = MathUtils::Clamp<float>(value3.posY, -0.5f, 0.5f);
                    
                    swimOrder = std::make_shared<Vector3>(value3);
                    turnOrder = Vector2(MathUtils::Clamp(x, -1.0f, 1.0f), 0.0f);
                }
                else if (componentLocomotion->walkSpeed > 0.0f)
                {
                    if (isTerrainSafeToGo(position, vector))
                    {
                        turnOrder = Vector2(MathUtils::Clamp(x, -1.0f, 1.0f), 0.0f);
                        if (num2 > 1.0f)
                        {
                            walkOrder = std::make_shared<Vector2>(0.0f, MathUtils::Lerp(speed, 0.0f, MathUtils::Saturate((MathUtils::Abs(x) - 0.33f) / 0.66f)));
                            if (speed >= 1.0f && componentCreature->componentLocomotion->inAirWalkFactor >= 1.0f && num > 1.0f && GameSingleton::gameRandom->getFloat(0.0f, 1.0f) < 0.05f)
                            {
                                jumpOrder = 1.0f;
                            }
                        }
                        else
                        {
                            float x2 = speed * MathUtils::Min(1.0f * MathUtils::Sqrt(num2), 1.0f);
                            walkOrder = std::make_shared<Vector2>(0.0f, MathUtils::Lerp(x2, 0.0f, MathUtils::Saturate(2.0f * MathUtils::Abs(x))));
                        }
                    }
                    else
                    {
                        isStuck = true;
                    }

                    componentCreature->componentBody->isSmoothRiseEnabled = (num2 >= 1.0f || vector.posY >= -0.1f);

                    if (num2 < 1.0f && (vector.posY < -0.5f || vector.posY > 1.0f))
                    {
                        if (vector.posY > 0.0f && GameSingleton::gameRandom->getFloat(0.0f, 1.0f) < 0.05f)
                        {
                            jumpOrder = 1.0f;
                        }
                        if (aboveBelowTime < 0.0f)
                        {
                            aboveBelowTime = timeOfDay->getGameTime();
                        }
                        else if (timeOfDay->getGameTime() - aboveBelowTime > 2.0 && componentBody->standingOnValue != 0)
                        {
                            isStuck = true;
                        }
                    }
                    else
                    {
                        aboveBelowTime = -1.0;
                    }
                }
                if ((ignoreHeightDifference == false ? (num) : (num2)) <= range * range)
                {
                    if (raycastDestination)
                    {
                        if (terrain->raycast(position + Vector3(0.0f, 0.5f, 0.0f), v + Vector3(0.0f, 0.5f, 0.0f), false, true, [](int32_t value, float distance) -> bool { return GameSingleton::blocksManager->blocks[Terrain::ExtractContents(value)]->IsCollidable; }) == nullptr)
                        {
                            destination = FastVector3::Zero;
                        }
                    }
                    else
                    {
                        destination = FastVector3::Zero;
                    }
                }
            }
            if (destination == FastVector3::Zero && 
                componentCreature->componentLocomotion->flySpeed > 0.0f && 
                componentCreature->componentBody->standingOnValue == 0 &&
                componentCreature->componentBody->immersionFactor == 0.0f)
            {
                turnOrder = FastVector2::Zero;
                walkOrder = nullptr;
                swimOrder = nullptr;
                flyOrder = std::make_shared<Vector3>(0.0f, -0.5f, 0.0f);
            }
        }
        componentCreature->componentLocomotion->walkOrder = CombineNullables(componentCreature->componentLocomotion->walkOrder, walkOrder);
        componentCreature->componentLocomotion->swimOrder = CombineNullables(componentCreature->componentLocomotion->swimOrder, swimOrder);
        componentCreature->componentLocomotion->turnOrder += turnOrder;
        componentCreature->componentLocomotion->flyOrder = CombineNullables(componentCreature->componentLocomotion->flyOrder, flyOrder);
        componentCreature->componentLocomotion->jumpOrder = MathUtils::Max(jumpOrder, componentCreature->componentLocomotion->jumpOrder);
        jumpOrder = 0.0f;

    }

    void ComponentPilot::setDestination(const Vector3 &theDestination, float theSpeed, float theRange, bool theIgnoreHeightDifference, bool theRaycastDestination, bool theTakeRisks, ComponentBody *theDoNotAvoidBody)
    {
        // Log::Info(fmt::format("ComponentPilot::setDestination {}", theDestination.toString()));

        bool flag = true;
        if(destination != FastVector3::Zero && theDestination != FastVector3::Zero)
        {
            Vector3 position;
            componentCreature->componentBody->getPosition(position);

            Vector3 v = Vector3::Normalize(destination - position);
            if(Vector3::Dot(Vector3::Normalize(theDestination - position), v) > 0.5f)
            {
                flag = false;
            }
        }
        if(flag)
        {
            isStuck = false;
            lastStuckCheckPosition = nullptr;
            aboveBelowTime = -1;
        }

        destination = theDestination;
        speed = theSpeed;
        range = theRange;
        ignoreHeightDifference = theIgnoreHeightDifference;
        raycastDestination = theRaycastDestination;
        takeRisks = theTakeRisks;
        doNotAvoidBody = theDoNotAvoidBody;
    }

    void ComponentPilot::stop()
    {
        setDestination(FastVector3::Zero, 0.0f, 0.0f, false, false, false, nullptr);
    }

    bool ComponentPilot::isTerrainSafeToGo(const Vector3& position, const Vector3 &direction)
    {
        Vector3 vector = position + Vector3(0.0f, 0.1f, 0.0f) + ((direction.lengthSquared() < 1.2f) ? Vector3(direction.posX, 0.0f, direction.posZ) : (1.2f * Vector3::Normalize(Vector3(direction.posX, 0.0f, direction.posZ))));
        SubsystemTerrain *terrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();

        for (int32_t i = -1; i <= 1; i++)
        {
            for (int32_t j = -1; j <= 1; j++)
            {
                if(!(Vector3::Dot(direction, Vector3(i, 0.0f, j)) > 0.0f))
                {
                    continue;
                }
                for (int32_t num = 0; num >= -2; num--)
                {
                    int32_t cellValue = terrain->getCellValue(Terrain::ToCell(vector.posX) + i, Terrain::ToCell(vector.posY) + num, Terrain::ToCell(vector.posZ) + j);
                    Block *block = GameSingleton::blocksManager->blocks[Terrain::ExtractContents(cellValue)];
                    if (block->shouldAvoid(cellValue))
                    {
                        return false;
                    }
                    if (block->IsCollidable)
                    {
                        break;
                    }
                }
            }
        }
        Vector3 Vector2 = position + Vector3(0.0f, 0.1f, 0.0f) + ((direction.lengthSquared() < 1.0f) ? Vector3(direction.posX, 0.0f, direction.posZ) : Vector3::Normalize(Vector3(direction.posX, 0.0f, direction.posZ)));
        bool flag = true;
        int32_t num2 = takeRisks ? 7 : 5;
        for (int32_t num3 = 0; num3 >= -num2; num3--)
        {
            int32_t cellValue2 = terrain->getCellValue(Terrain::ToCell(Vector2.posX), Terrain::ToCell(Vector2.posY) + num3, Terrain::ToCell(Vector2.posZ));
            Block *block2 = GameSingleton::blocksManager->blocks[Terrain::ExtractContents(cellValue2)];
            if ((block2->IsCollidable || block2->getIndex() == WaterBlock::Index()) && false == block2->shouldAvoid(cellValue2))
            {
                flag = false;
                break;
            }
        }
        if (flag)
        {
            return false;
        }
        return true;
    }

    ComponentBody *ComponentPilot::findNearestBodyInFront(const Vector3& position, const Vector2 &direction)
    {
        SubsystemTimeOfDay *timeOfDay = GameSingleton::singletonPtr->getSingleton<SubsystemTimeOfDay>();
        SubsystemBodies *bodies = GameSingleton::singletonPtr->getSingleton<SubsystemBodies>();

        Vector3 bodyPosition;
        componentCreature->componentBody->getPosition(bodyPosition);

        
        // if (timeOfDay->getGameTime() >= nextBodiesUpdateTime)
        {
            nextBodiesUpdateTime = timeOfDay->getGameTime() + 0.5f;
            nearbyBodies.clear();
            bodies->findBodiesAroundPoint(Vector2(bodyPosition.posX, bodyPosition.posZ), 4.0f, nearbyBodies);
        }
        ComponentBody *result = nullptr;
        float num = FLT_MAX;
        for (ComponentBody *nearbyBody : nearbyBodies)
        {
            Vector3 nearByBodyPosition;
            nearbyBody->getPosition(nearByBodyPosition);

            if (nearbyBody != componentCreature->componentBody && false == (MathUtils::Abs(nearByBodyPosition.posY - bodyPosition.posY) > 1.1f) && Vector2::Dot(Vector2(nearByBodyPosition.posX - position.posX, nearByBodyPosition.posZ - position.posZ), direction) > 0.0f)
            {
                float num2 = Vector2::DistanceSquared(Vector2(nearByBodyPosition.posX, nearByBodyPosition.posZ), Vector2(position.posX, position.posZ));
                if (num2 < num)
                {
                    num = num2;
                    result = nearbyBody;
                }
            }
        }
        return result;
    }

    Vector3 ComponentPilot::avoidNearestBody(const Vector3 &position, const Vector3 &destination)
    {
        Vector2 v(destination.posX - position.posX, destination.posZ - position.posZ);
        ComponentBody *componentBody = findNearestBodyInFront(position, Vector2::Normalize(v));
        if (componentBody != nullptr && componentBody != doNotAvoidBody)
        {
            float num = 0.72f * (componentBody->boxSize.posX + componentCreature->componentBody->boxSize.posX) + 0.5f;

            Vector3 position;
            componentBody->getPosition(position);

            Vector2 xZ(position.posX, position.posZ);
            Vector2 v2 = Segment2::NearestPoint(Segment2(Vector2(position.posX, position.posZ), Vector2(destination.posX, destination.posZ)), xZ) - xZ;
            if (v2.lengthSquared() < num * num)
            {
                float num2 = v.length();
                auto v3 = Vector2::Normalize(xZ + Vector2::Normalize(v2) * num - Vector2(position.posX, position.posZ));
                if (Vector2::Dot(v / num2, v3) > 0.5f)
                {
                    return Vector3(position.posX + v3.posX * num2, destination.posY, position.posZ + v3.posY * num2);
                }
            }
        }
        return destination;
    }



    static std::shared_ptr<Vector2> CombineNullables(std::shared_ptr<Vector2> &v1, std::shared_ptr<Vector2> &v2)
    {
        if (v1 == nullptr)
        {
            return v2;
        }
        if (v2 == nullptr)
        {
            return v1;
        }
        return std::make_shared<Vector2>((*v1) + (*v2));
    }

    static std::shared_ptr<Vector3> CombineNullables(std::shared_ptr<Vector3> &v1, std::shared_ptr<Vector3> &v2)
    {
        if (v1 == nullptr)
        {
            return v2;
        }
        if (v2 == nullptr)
        {
            return v1;
        }
        return std::make_shared<Vector3>((*v1) + (*v2));
    }
    

    static const std::string componentName = "Pilot";
    const std::string &ComponentPilot::getName() const
    {
        return componentName;
    }
    const std::string &ComponentPilot::GetName()
    {
        return componentName;
    }
    void ComponentPilot::load(const nlohmann::json &jsonData)
    {
        componentCreature = GameSingleton::gameRegistry->get<ComponentCreature*>(entity);
    }

}

